#include "DemoWidget.h"
#include <Poco/Thread.h>

#pragma region Init
DemoWidget::DemoWidget(QWidget *parent) : QWidget(parent)
{
    // 初始化可视化界面
    pcl_viewer = std::make_shared<pcl::visualization::PCLVisualizer>("3D Viewer");
    pcl_viewer->setBackgroundColor(0, 0, 0);
    pcl_viewer->addCoordinateSystem(1.0);
    pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_pointcloud(new pcl::PointCloud<pcl::PointXYZI>);
    pcl_viewer->addPointCloud<pcl::PointXYZI>(pcl_pointcloud, "pointcloud");
    pcl_viewer->setPointCloudRenderingProperties(PCL_VISUALIZER_POINT_SIZE, 2, "pointcloud");

    timer = new QTimer(this);
    connect(timer, &QTimer::timeout, this, &DemoWidget::updateViewer);
    timer->start(100);

    // 启动lidar
    _dataManager = LidarDataManager::getInstance();
    std::vector<LidarConfigurationOptions *> _options;
    LidarConfigurationOptions *_option1 = new LidarConfigurationOptions();
    _option1->SetValue(LidarConfigurationOptions::Data_Source_SrcType, LidarInputSourceType::ONLINE_LIDAR);
    _option1->SetValue(LidarConfigurationOptions::Data_SrcNet_MsopPort, (uint16_t)6699);
    _option1->SetValue(LidarConfigurationOptions::Data_SrcNet_DifopPort, (uint16_t)7788);
    _option1->SetValue(LidarConfigurationOptions::Data_SrcNet_ManuType, LidarManufacturerType::RS_M1);
    // TransformParam _transform1;
    // _transform1.yaw = M_PI / 2;
    // _option1->SetValue(LidarConfigurationOptions::Data_Decode_Transform, _transform1);
    _options.push_back(_option1);
    _dataManager->UseOptions(_options);

    _dataManager->addPointCloudMsgCallback(std::bind(&DemoWidget::processData, this, std::placeholders::_1));
    _dataManager->start();
}

DemoWidget::~DemoWidget()
{
    _dataManager->stop();
    delete _dataManager;
}
#pragma endregion

#pragma region Process
bool DemoWidget::processData(PointCloudData<pcl::PointXYZI> &data)
{
    auto size = data.cloud_ptr->size();
    auto timestamp = QString::number(data.timestamp, 'f', 2);

    std::cout << timestamp.toStdString() << "," << size << std::endl;

    {
        std::lock_guard<std::mutex> lock(mtx_viewer);
        cloud_queue.push(data);
        if (cloud_queue.size() > 2)
            cloud_queue.pop();
    }
    return true;
}
#pragma endregion

#pragma region Update Viewer
void DemoWidget::updateViewer()
{
    if (cloud_queue.size() > 0)
    {
        PointCloudData<pcl::PointXYZI> pointcloud;
        {
            std::lock_guard<std::mutex> lock(mtx_viewer);
            pointcloud = cloud_queue.front();
            cloud_queue.pop();
        }
        pcl_viewer->removePointCloud("pointcloud");
        pcl_viewer->addPointCloud<pcl::PointXYZI>(pointcloud.cloud_ptr, "pointcloud");
        pcl_viewer->spinOnce();
    }
}
#pragma endregion